function X = triangulate(P0,P1,x0,x1)
%TRIANGULATE Triangulate a point given two cameras and their projections
%   Given two camera matrices of the form P = K*M, M = [R | -RC],
% and two homogenous pixels of a projected point with co-ordinates 
% p = [x y w] (or [u v 1]), returns the trinagulated homogenous point
% X = [X Y Z 1]

% Copyright (c) Michael Warren 2014

% This file is part of the AMMCC Toolbox.

% The AMMCC Toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% The AMMCC Toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Lesser General Public License
% along with the AMMCC Toolbox.  If not, see <http://www.gnu.org/licenses/>.

x0 = x0./x0(3); % normalise
x1 = x1./x1(3); % normalise

Z(1,:) = x0(1)*P0(3,:) - P0(1,:);
Z(2,:) = x0(2)*P0(3,:) - P0(2,:);
Z(3,:) = x1(1)*P1(3,:) - P1(1,:);
Z(4,:) = x1(2)*P1(3,:) - P1(2,:);
[U,S,V] = svd(Z);

X = V(:,4);
X = X./X(4); % normalise

return